#ifndef __RGM_H__
#define __RGM_H__

template <class T, class U>
int connect(const pip::Image<T> &in,
            pip::Image<U> &out,
            const U &target, // Value for connected object in out
            const int &n,    // Connectivity
            const int &x,    // seed coordinates
            const int &y,
            const int &z = 0 // Optional
            )
{
  // If out is not the same size as in,
  // resize it and fill with 0.
  if (out.dimension()!=in.dimension()) {
    out.resize(in.dimension());
    out.fill(U(0));
  }

  long connected(1);     // how many pixels have been connected?  at least the given seed... :)
  Pixel seed(x, y, z);
  T val = seed.get(in);  // the value for comparison
  seed.set(out, target); // mark the seed - we have counted this pixel

  std::deque<Pixel> q(1, seed); // seed into deque so we can enter the while-loop 
  std::vector<Pixel> nn;         //vector storing the neighbour 
  Pixel i;                      // temp pixel
  while (!q.empty()) {
    i = q.front();               // get first element from deque and work on it 
    i.getNeighbors(in, n, nn);   //get its neighbours

    for (std::vector<Pixel>::iterator j = nn.begin(); j != nn.end(); ++j) { //go through all neigbours
      if ((j->get(in) == val) && (j->get(out) != target)) {      //if they are in the same object
		connected++;                                 //count them
        q.push_back(*j);                     // push them in the deque
        j->set(out, target);                          // and mark them
      }
    }
    q.pop_front();                                   // remove first element from deque
  }
  return connected; // if the deque is empty, all elements were handeled... we can return the result
}



template <class T> 
void connectedComponentLabeling(
const pip::Image<T> &in, // input image
pip::Image<float> &out, // output image
int n, // connectivity
std::vector<int> &sizes // returns the sizes of the objects
)

{
  // If out is not the same size as in,
  // resize it and fill with 0.
  if (out.dimension()!=in.dimension()) {
    out.resize(in.dimension());
    out.fill(float(0));      // and zero means pixel has not been connected yet...
  }

  T val(0);
  float target(1);
  int connected(1),x(0),y(0),z(0);

  std::deque<Pixel> now(1, Pixel(x,y,z));
  std::deque<Pixel> later;                  // instead of running through output-finding the next '0' i will push pixels in a deque to remember them
  // I know there could be problems if many pixels have to be saved, but it saves some time.
  std::vector<Pixel> nn;   
  Pixel i,j;
  do{
	  while (!now.empty()) {
		i = now.front();
		val=i.get(in);                  // work on the current head of the deque 
		i.set(out, target);
		i.getNeighbors(in, n, nn);

		for (std::vector<Pixel>::iterator j = nn.begin(); j != nn.end(); ++j) {
		  if ((j->get(in) == val) && (j->get(out) == 0)) {
			connected++;                     // and find for all it's neighbours elements of the same object
			now.push_back(*j);
			j->set(out, target);
		  }
		  else if(j->get(out) == 0)
		  {
			later.push_back(*j);     // if they are not from the same object, we will handle them after this object
		  }
		}
		now.pop_front();          
	  }   // yet all pixels of this object are done: 'now' empty...

	  while(now.empty())      // as 'now' is empty we will fill it with one new seed from the deque 'later'...
	  {
		  if(!later.empty()){
	  		if( later.front().get(out) == 0)  // only with a pixel which is not in a object already
	  		{
				later.front().get(x,y,z);       
				now.push_back(Pixel(x,y,z));
			}
			later.pop_front();
		  }
		  else
		  {
			break;
		  }
	  }

	  ++target;            // as we are going to do a new object we need a new target value...
	  sizes.push_back(connected);  // and we can save the result of the last object
	  connected = 1;
  }while(!now.empty());                  // if 'later' was empty ... 'now is now empty, too ...
  std::sort(sizes.begin(), sizes.end());  // in the end we will sort the results 
}



template <class T, class U, class GrowCheck>
int grow(
const pip::Image<T> &in,
pip::Image<U> &out,
GrowCheck gc,
U target, // value for connected object in out
int n, // connectivity
int x, int y,
int z = 0 // optional
)
{
    //resize out, if we must
    if (out.dimension()!=in.dimension()) {
            out.resize(in.dimension());
            out.fill(U(0)); 
    }
	
	int connected(1);     
    Pixel seed(x,y,z);
    seed.set(out,target);
    vector<Pixel> nn;
    deque<Pixel> q;
    q.push_back(seed);
    while (!q.empty()) {
            deque<Pixel>::iterator i = q.begin();
            i->getNeighbors(in,n,nn);
            for (vector<Pixel>::iterator j=nn.begin();j!=nn.end();++j) {
                    if (gc(j->get(in))&&(j->get(out)!=target)) {   // watch out: gc is a growcheck (class) - passed as argument...
						    ++connected;                           // we check for all neigbours that fit the 'grow' criterion...
                            q.push_back(*j);
                            j->set(out,target);
                    }
            }
            q.pop_front();
    }
    return connected;  // we count their number
};


template <class T, class U>
int regionGrowUp(
const pip::Image<T> &in,
pip::Image<U> &out,
T threshold,
U target, // value for connected object in out
int n, // connectivity
int x, int y,
int z = 0 // optional
)
{
    RegionGrowUpCheck<T> gc(threshold);      //to call the grow function we need a grow-check... for UP_growing we need a 'up-check'
	return grow(in,out,gc,target,n,x,y,z);
};

template <class T, class U>
int regionGrowDown(
const pip::Image<T> &in,
pip::Image<U> &out,
T threshold,
U target, // value for connected object in out
int n, // connectivity
int x, int y,
int z = 0 // optional
)
{
    RegionGrowDownCheck<T> gc(threshold);   // here we pass a DOWN_growing criterion
	return grow(in,out,gc,target,n,x,y,z); 
};




template <class T>
class ConnectCheck
{
        private:
        T t;
        public:
        ConnectCheck(const T &val) : t(val) {}             
        bool operator()(const T &val) {return val==t;}
};


template <class T>
class RegionGrowUpCheck 
{
        private:
        T t;
        public:
        RegionGrowUpCheck (const T &val) : t(val) {}  //constructor: get a value 
        bool operator()(const T &val) {return val<=t;}     //only function... return true if call_argument is smaller(equal) than value!
};

template <class T>
class RegionGrowDownCheck 
{
        private:
        T t;
        public:
        RegionGrowDownCheck (const T &val) : t(val) {} // same for the down direction
        bool operator()(const T &val) {return val>=t;} 
};


template <class T, class U>
int connect2(
const pip::Image<T> &in,
pip::Image<U> &out,
U target, // value for connected object in out
int n, // connectivity
int x, int y,
int z = 0 // optional
)
{
        ConnectCheck<T> gc(in(x,y,z));          // I its the 'connect' function with a grow-check class...
        return grow(in,out,gc,target,n,x,y,z);
}



template <class T, class U>
int regionGrow(
const pip::Image<T> &in,
pip::Image<U> &out,
T threshold,
U target,
int n,
int x, int y,
int z = 0)                               // just copied
{
        T val = in(x,y,z);
        if (val<=threshold) {
                cout << "Value at start point is " << float(val) << ", Threshold is " << float(threshold)
                << " so everything below or equal to the threshold will be grown." << endl;
                return regionGrowUp(in,out,threshold,target,n,x,y,z);
        }
        else {
                cout << "Value at start point is " << float(val) << ", Threshold is " << float(threshold)
                << " so everything above or equal to the threshold will be grown." << endl;
                return regionGrowDown(in,out,threshold,target,n,x,y,z);
        }
}


template <class T> 
void hysteresisThresholding(
const pip::Image<T> &in, // input image
pip::Image<unsigned char> &out, // output image
int n, // connectivity
T tHigh,
T tLow
)
{
  // If out is not the same size as in,
  // resize it and fill with 0.
  if (out.dimension()!=in.dimension()) {
    out.resize(in.dimension());      // and zero means pixel has not been connected yet...
  }
  out.fill(unsigned char(0));        //anyways make the output-blanc ... important to avoid side-effects of calling the function several times on the same 'out' image

  unsigned char target(1);           
  int x(0),y(0),z(0);

  std::deque<Pixel> growThem;    // these pixel have to be grown with the second threshold, since they are ...
  std::vector<Pixel> nn;

  Pixel i;

  for(int cx=0; cx < in.dim(0);++cx){
	  for(int cy=0;cy < in.dim(1);++cy){
		  for(int cz=0;cz < in.dim(2);++cz){
			if(in(cx,cy,cz)> tHigh)
				out(cx,cy,cz)=target;
				Pixel(cx,cy,cz).getNeighbors(in, n, nn);
				for (std::vector<Pixel>::iterator j = nn.begin(); j != nn.end(); ++j) {
					growThem.push_back(*j);                 // ... neigbours of pixel which passed the first threshold
				}
		  }
	  }
  }
  while (!growThem.empty()) {          // those pixel are grwoing down...
	    i = growThem.front();
		i.get(x,y,z);
		if(i.get(in)>tLow)             // as long as they are greater than the lower threshold
			regionGrowDown(in,out,tLow,target,n,x,y,z);
		growThem.pop_front();
  }		    				
}
	




class Pixel
{
private:
  int _x, _y, _z;

public:
  Pixel(const int &x = 0, const int &y = 0, const int &z = 0) : _x(x), _y(y), _z(z) {}
  Pixel(const Pixel &pixel) : _x(pixel._x), _y(pixel._y), _z(pixel._z) {}

  template <class T>
  T get(const pip::Image<T> &image) const { return image(_x, _y, _z); }

  template <class T>
  void set(pip::Image<T> &image, const T &val) const { image(_x, _y, _z) = val; }

  void get(int &x, int &y) const { x = _x; y = _y; }
  void get(int &x, int &y, int &z) const { x = _x; y = _y; z = _z; }

  void set(const int &x, const int &y, const int &z = 0) { _x = x; _y = y; _z = z; }

  template <class T>
  void getNeighbors(const pip::Image<T> &im, const int &n, std::vector<Pixel> &v) const
  {
	     

	  int dim0(im.dim(0)),dim1(im.dim(1)),dim2(im.dim(2));

	  if((n==4) || // if connectivity is valid
		 (n==6) || 
		 (n==8) || 
		 (n==18)|| 
		 (n==26))
	  {
	   	  v.resize(0);
	      if(!(dim2>1))  // if 2 dimensional image
		  {
			  // Arrays with offsets to 2D neighborhood.
			  // Taking the first 4 gives 4-neighborhood, taking all gives 8-neighborhood.
			  const int dnbx2D[8] = {1+_x, 0+_x,-1+_x, 0+_x, 1+_x, 1+_x,-1+_x,-1};
			  const int dnby2D[8] = {0+_y, 1+_y, 0+_y,-1+_y, 1+_y,-1+_y, 1+_y,-1};
			  for(int i=0; i < n; ++i)
			  {
				  if((dim0 > dnbx2D[i] && dnbx2D[i] >= 0) && (dim1 > dnby2D[i] && dnby2D[i] >= 0))  // if not out of image bounds
					   v.push_back(Pixel(dnbx2D[i], dnby2D[i]));     // this is a neighbour
			  }
		  }
		  else  // if 3D
		  {
			  // Arrays with offsets to 3D neighborhood.
			  // Taking the first 6 gives 6-neighborhood, taking first 18 gives 18-neighborhood,
			  // taking all gives 26 neighborhood.
			  const int dnbx3D[26] = {1+_x, 0+_x, 0+_x,-1+_x, 0+_x, 0+_x,   1+_x, 1+_x, 1+_x, 1+_x, 0+_x, 0+_x,-1+_x,-1+_x,-1+_x,-1+_x, 0+_x, 0+_x,   1+_x, 1+_x, 1+_x,-1+_x,-1+_x,-1+_x, 1+_x,-1};
			  const int dnby3D[26] = {0+_y, 1+_y, 0+_y, 0+_y,-1+_y, 0+_y,   1+_y,-1+_y, 0+_y, 0+_y, 1+_y, 1+_y, 1+_y,-1+_y, 0+_y, 0+_y,-1+_y,-1+_y,   1+_y,-1+_y, 1+_y, 1+_y, 1+_y,-1+_y,-1+_y,-1};
			  const int dnbz3D[26] = {0+_z, 0+_z, 1+_z, 0+_z, 0+_z,-1+_z,   0+_z, 0+_z, 1+_z,-1+_z, 1+_z,-1+_z, 0+_z, 0+_z, 1+_z,-1+_z, 1+_z,-1+_z,   1+_z, 1+_z,-1+_z, 1+_z,-1+_z, 1+_z,-1+_z,-1};
			  for(int i=0; i < n; ++i)
			  {
				  if((dim0 > dnbx3D[i]     // if not out of image bounds
				  && dnbx3D[i] >= 0) 
					  && (dim1 > dnby3D[i] 
				  && dnby3D[i] >= 0) 
					  && (dim2 > dnbz3D[i] 
				  && dnbz3D[i] >= 0))
					   v.push_back(Pixel(dnbx3D[i], dnby3D[i], dnbz3D[i]));  // remember this neigbour
			  }

		  }
	  }
	  else  // if not valid connectivity
	  {
		  std::cerr << "Pixel.getNeighbors: wrong neighbour number\n";
	  }
  }
};


template <class T> 
void solveable(     // is a maze solveable??? 
const pip::Image<T> &in, // input image
pip::Image<unsigned char> &out, // output image
int n
)

{
  // If out is not the same size as in,
  // resize it and fill with 0.
  if (out.dimension()!=in.dimension()) {
    out.resize(in.dimension());
    out.fill(unsigned char(0));      // and zero means pixel has not been connected yet...
  }

  unsigned char target(1); 
  int x(0),y(0),z(0);

  std::deque<Pixel> now;
  std::vector<Pixel> nn;
  Pixel i;
  for(int cx=0;cx < in.dim(0);++cx){
	  for(int cy=0;cy < in.dim(1);++cy){
		  if(in(cx,cy)==unsigned char(102)){  // if any pixel in image is 102
			  now.push_back(Pixel(cx,cy));    // add it to the 'path'
		  }
	  }
  }

  while (!now.empty())   // as long as there is still 'path' left to go
  {
	i = now.front();
	i.set(out, target);
	i.getNeighbors(in, n, nn); // explore further 'path'

	for (std::vector<Pixel>::iterator j = nn.begin(); j != nn.end(); ++j)
	{
	  if ( j->get(in)==unsigned char(255) && (j->get(out) == unsigned char(0)))
	  {
		now.push_back(*j);         // go the 'path' further
		j->set(out, target);       // show footprints on the 'path'
	  }
	  else if(j->get(in)==unsigned char(175))
	  {
		  std::cout << "this maze is solveable!\n"; // if goal is on 'path': report ... 
			  return;
	  }
	}
	now.pop_front();
  }
  std::cout << "this maze is NOT solveable!\n";  // the goal wasn't on path
}


#endif